/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: PositionData.cpp,v 1.15 2009-11-02 09:35:21 morsko Exp $
 *
 * \file
 * \brief Implementatino for Position data encoder/decoder
 * \author Antti Maula <antti.maula@tkk.fi>
 */
#include "PositionData.hpp"
#include "binbag.h"
#include <stdio.h>
#include "owndebug.h"

//*****************************************************************************
//*****************************************************************************
using namespace MaCI::Position;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CPositionData::CPositionData()
  : iTimestampPtr(NULL),
    iPose2DPtr(NULL),
    iPose3DPtr(NULL),
    iVariance2DPtr(NULL),
    iVariance3DPtr(NULL),
    iProbabilityPtr(NULL),
    iPose2DReferencePtr(NULL),
    iCommandPtr(NULL),
    iSpeed2DPtr(NULL),
    iPose2DProjectionFrom3D(),
    iVariance2DProjectionFrom3D()
{
}
//*****************************************************************************

CPositionData::~CPositionData()
{
}
//*****************************************************************************

CPositionData::CPositionData(const CPositionData &aData)
  : iTimestampPtr(NULL),
    iPose2DPtr(NULL),
    iPose3DPtr(NULL),
    iVariance2DPtr(NULL),
    iVariance3DPtr(NULL),
    iProbabilityPtr(NULL),
    iPose2DReferencePtr(NULL),
    iCommandPtr(NULL),
    iSpeed2DPtr(NULL),
    iPose2DProjectionFrom3D(),
    iVariance2DProjectionFrom3D()
{
  // Call assignment operator.
  *this = aData;
}
//*****************************************************************************

CPositionData &CPositionData::operator=(const CPositionData &aData)
{
  // Reset
  Reset();

  // Create an internal BinBag instance
  if (CreateInternalBinBag(aData.iBinBagContainer) == false) {
    dPrint(4,"WARNING: Failed to create internal binbag for object %p from object %p",
           this, &aData);
  }
  
  // Do the decode on the buffer; it sets pointers and
  if (DecodeFrom() == false) {
    dPrint(4,"Warning: Failed to Decode CPositionData even though assignment operator used?");
  }
  
  return *this; 
}
//*****************************************************************************

bool CPositionData::DecodeFrom(gim::binbag::CBinBag *aBinBag)
{
  bool result = true;
  const gim::binbag::TBinBlob *b = NULL;
  int previous_type = -1;
  
  // Store pointer and test it for NullNess
  if (aBinBag != NULL)  
    iBinBagContainer = aBinBag;
  
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    
    while( (b = iBinBagContainer->GetNextBinary()) && result ) {
      switch(b->type) {
      case KTypeTimestamp:
	if (iTimestampPtr == NULL) {
	  iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);
	} else { 
          dPrint(10,"Decoding Error: iTimestampPtr was not NULL");
          result = false; 
        }
	break;

      case KTypePose2D:
	if (iPose2DPtr == NULL) {
	  iPose2DPtr = reinterpret_cast<const TPose2D *>(b->data);
	} else { 
          dPrint(10,"Decoding Error: iPose2DPtr was not NULL");
          result = false; 
        }
        break;
      case KTypePose3D:
	if (iPose3DPtr == NULL) {
	  iPose3DPtr = reinterpret_cast<const TPose3D *>(b->data);
	} else { 
          dPrint(10,"Decoding Error: iPose3DPtr was not NULL");
          result = false; 
        }
        break;
      case KTypeVariance2D:
	if (iVariance2DPtr == NULL) {
	  iVariance2DPtr = reinterpret_cast<const TVariance2D *>(b->data);

	} else { 
          dPrint(10,"Decoding Error: iVariance2DPtr was not NULL");
          result = false; 
        }
        break;
      case KTypeVariance3D:
	if (iVariance3DPtr == NULL) {
	  iVariance3DPtr = reinterpret_cast<const TVariance3D *>(b->data);

	} else { 
          dPrint(10,"Decoding Error: iVariance3DPtr was not NULL");
          result = false; 
        }
        break;
      case KTypeProbability:
	if (iProbabilityPtr == NULL) {
	  iProbabilityPtr = reinterpret_cast<const TProbability *>(b->data);
	} else { 
          dPrint(10,"Decoding Error: iProbabilityPtr was not NULL");
          result = false; 
        }
        break;

      case KTypePose2DReference:
	if (iPose2DReferencePtr == NULL) {
	  iPose2DReferencePtr = reinterpret_cast<const TPose2DReference *>(b->data);
	} else { 
          dPrint(10,"Decoding Error: iPose2DReferencePtr was not NULL");
          result = false; 
        }
        break;

      case KTypeSpeed2D:
	if (iSpeed2DPtr == NULL) {
	  iSpeed2DPtr = reinterpret_cast<const TSpeed2D *>(b->data);
	} else { 
          dPrint(10,"Decoding Error: iSpeed2DPtr was not NULL");
          result = false; 
        }
        break;

      case KTypeCommand:
        if (iCommandPtr == NULL) {
          iCommandPtr = reinterpret_cast<const TCommand *>(b->data);
        } else { 
          dPrint(10,"Decoding Error: iCommandPtr was not NULL");
          result = false; 
        }
        break;

      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
      
      // Store previous type
      previous_type = b->type;
    }
  } else {
    result = false;
  }
  
  // Store the result
  SetReadOnlyContainer(result);
  
  return result;
}
//*****************************************************************************

void CPositionData::Reset()
{ 
  CMaCIData::Reset();

  iTimestampPtr = NULL;
  iPose2DPtr = NULL;
  iPose3DPtr = NULL;
  iVariance2DPtr = NULL;
  iVariance3DPtr = NULL;
  iProbabilityPtr = NULL;
  iCommandPtr = NULL;
  iSpeed2DPtr = NULL;
}
//*****************************************************************************

bool CPositionData::SetTimestamp(const Common::TTimestamp &aStamp)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iTimestampPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeTimestamp, 
                                  (const char *)&aStamp, 
                                  sizeof(Common::TTimestamp));
    iTimestampPtr = reinterpret_cast<const Common::TTimestamp *>(b->data);

    // Mark last element
    SetLastElementType(KTypeTimestamp);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CPositionData::SetPose2D(const TPose2D &aPose2D)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iPose2DPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypePose2D,
                                  (const char *)&aPose2D, 
                                  sizeof(TPose2D));
    iPose2DPtr = reinterpret_cast<const TPose2D *>(b->data);

    // Mark last element
    SetLastElementType(KTypePose2D);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CPositionData::SetPose3D(const TPose3D &aPose3D)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iPose3DPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypePose3D,
                                  (const char *)&aPose3D, 
                                  sizeof(TPose3D));
    iPose3DPtr = reinterpret_cast<const TPose3D *>(b->data);

    // Mark last element
    SetLastElementType(KTypePose3D);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CPositionData::SetVariance2D(const TVariance2D &aVariance2D)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iVariance2DPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeVariance2D,
                                  (const char *)&aVariance2D, 
                                  sizeof(TVariance2D));
    iVariance2DPtr = reinterpret_cast<const TVariance2D *>(b->data);

    // Mark last element
    SetLastElementType(KTypeVariance2D);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CPositionData::SetVariance3D(const TVariance3D &aVariance3D)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iVariance3DPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeVariance3D,
                                  (const char *)&aVariance3D, 
                                  sizeof(TVariance3D));
    iVariance3DPtr = reinterpret_cast<const TVariance3D *>(b->data);

    // Mark last element
    SetLastElementType(KTypeVariance3D);

    result = true;
  }
  return result;
}
//*****************************************************************************

bool CPositionData::SetProbability(const TProbability &aProbability)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iProbabilityPtr == NULL) {
    const gim::binbag::TBinBlob *b = 
      iBinBagContainer->AddBinary(KTypeProbability,
                                  (const char *)&aProbability,
                                  sizeof(TProbability));
    iProbabilityPtr = reinterpret_cast<const TProbability *>(b->data);
    
    // Mark last element
    SetLastElementType(KTypeProbability);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CPositionData::SetPose2DReference(const TPose2DReference &aRef)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iPose2DReferencePtr == NULL) {
    ADDELEMENT(TPose2DReference, KTypePose2DReference, iPose2DReferencePtr, aRef);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CPositionData::SetCommand(const TCommand &aCommand)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iCommandPtr == NULL) {
    // Use the handy Macro found from MaCIData
    ADDELEMENT(TCommand, KTypeCommand, iCommandPtr, aCommand);
    result = true;
  }
  return result;
}
//*****************************************************************************

bool CPositionData::SetSpeed2D(const TSpeed2D &aSpeed2D)
{
  bool result = false;
  if (!IsReadOnlyContainer() && iSpeed2DPtr == NULL) {
    ADDELEMENT(TSpeed2D, KTypeSpeed2D, iSpeed2DPtr, aSpeed2D);
    result = true;
  }
  return result;
}
//*****************************************************************************

void CPositionData::Print(const int l) const
{
 
  const gim::binbag::TBinBlob *b = NULL;
  // Parse
  if (iBinBagContainer && iBinBagContainer->Rewind()) {
    while( (b = iBinBagContainer->GetNextBinary())) {
      switch(b->type) {
      case KTypeTimestamp: {
	const Common::TTimestamp *ts = 
          reinterpret_cast<const Common::TTimestamp *>(b->data);
	dPrint(l,"Common::TTimestamp : %d, %d", 
	       ts->timestamp_s,
	       ts->timestamp_us);
	break;
      }

      case KTypePose2D: {
	const TPose2D *tp = 
          reinterpret_cast<const TPose2D *>(b->data);
	dPrint(l,"TPose2D: x:%f, y:%f, heading:%f",
	       tp->x,
	       tp->y,
               tp->a);
        break;
      }

      case KTypePose3D: {
	const TPose3D *tp = 
          reinterpret_cast<const TPose3D *>(b->data);
	dPrint(l,"TPose3D: x:%f, y:%f, z:%f, eul_x:%f, eul_y:%f, eul_z:%f",
	       tp->x,
	       tp->y,
               tp->z,
               tp->eul_x,
               tp->eul_y,
               tp->eul_z);
        break;
      }

      case KTypeVariance2D: {
	const TVariance2D *tpv = 
          reinterpret_cast<const TVariance2D *>(b->data);
	dPrint(l,"TVariance2D: var_x:%f, var_y:%f, var_heading:%f",
	       tpv->x,
	       tpv->y,
               tpv->a);
        break;
      }
      case KTypeVariance3D: {
	const TVariance3D *tpv = 
          reinterpret_cast<const TVariance3D *>(b->data);
	dPrint(l,"TVariance3D: var_x:%f, var_y:%f, var_z:%f, var_angleX:%f, var_angleY:%f, var_angleZ:%f",
	       tpv->x,
	       tpv->y,
               tpv->z,
               tpv->eul_x,
               tpv->eul_y,
               tpv->eul_z);
        break;
      }
        
      case KTypeProbability: {
	const TProbability *tp = 
          reinterpret_cast<const TProbability *>(b->data);
	dPrint(l,"TProbability: %f",
	       tp->probability);
        break;
      }

      case KTypePose2DReference: {
	const TPose2DReference *tp = 
          reinterpret_cast<const TPose2DReference *>(b->data);
        const char reftabstr[4][9] = {
          "Unknown",
          "Odometry",
          "SLAM",
          "Map" };
        
	dPrint(l,"TPose2DReference: %d ('%s')",
	       tp->ref, reftabstr[ (tp->ref >= 0 && tp->ref < 4) ? tp->ref : 0 ]);
        break;
      }

      case KTypeCommand: {
        const TCommand *cp = reinterpret_cast<const TCommand *>(b->data);
        dPrint(l,"TCommand: %d",
               cp->cmd);
        break;
      }
	
      case KTypeSpeed2D: {
        const TSpeed2D *sp = reinterpret_cast<const TSpeed2D *>(b->data);
        dPrint(l,"TSpeed2D: speed %f, angular_speed %f",
               sp->speed,
               sp->angularSpeed);
        break;
      }
      default:
	// Unknown Type in BinBag. Do nothing.
	break;
      }
    }
  }
}
//*****************************************************************************

const TPose2D *CPositionData::GetPose2D(void) const {
  if(iPose2DPtr){
    return iPose2DPtr;
  }else if(iPose3DPtr){
    iPose2DProjectionFrom3D.x = iPose3DPtr->x;
    iPose2DProjectionFrom3D.y = iPose3DPtr->y;
    iPose2DProjectionFrom3D.a = iPose3DPtr->eul_z;
    return (const TPose2D *)(&iPose2DProjectionFrom3D);
  }else{
    return iPose2DPtr; ///< NULL
  }
}
//*****************************************************************************
const TVariance2D *CPositionData::GetVariance2D(void) const {
  if(iVariance2DPtr){
    return iVariance2DPtr;
  }else if(iVariance3DPtr){
    iVariance2DProjectionFrom3D.x = iVariance3DPtr->x;
    iVariance2DProjectionFrom3D.y = iVariance3DPtr->y;
    iVariance2DProjectionFrom3D.a = iVariance3DPtr->eul_z;
    return (const TPose2D *)(&iVariance2DProjectionFrom3D);
  }else{
    return iVariance2DPtr; ///< NULL
  }
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

#ifdef COMPILE_POSITIONDATA_STANDALONE_MAIN
using namespace MaCI::Position;

int main(void)
{
  gim::binbag::CBinBag bb;
  CPositionData A;
  CPositionData B;

  printf("Carry out Encoding/Decoding test.\n");

  printf("Encoding...\n");
  // Do Encoding 
  A.EncodeTo(&bb);
  printf("Encode: Timestamp(1,2)...");
  A.SetTimestamp(MaCI::Common::TTimestamp(1, 2));
  printf("OK.\n");

  printf("Encode: TPose2D(1.0, 2.0, 3.0)...");
  A.SetPose2D(TPose2D(1.0, 2.0, 3.0));
  printf("OK.\n");

  printf("Encode: TPose3D(1.0, 2.0, 3.0, 4.0, 5.0, 6.0)...");
  A.SetPose3D(TPose3D(1.0, 2.0, 3.0,4.0,5.0,6.0));
  printf("OK.\n");

  printf("Encode: TVariance2D(0.1, 0.2, 0.3)...");
  A.SetVariance2D(TVariance2D(0.1, 0.2, 0.3));
  printf("OK.\n");

  printf("Encode: TVariance3D(0.1, 0.2, 0.3, 0.11, 0.12, 0.13)...");
  A.SetVariance3D(TVariance3D(0.1, 0.2, 0.3, 0.11, 0.12, 0.13));
  printf("OK.\n");

  printf("Encode: TCommand(KCommandInitialize)...");
  A.SetCommand(TCommand(KCommandInitialize));
  printf("OK.\n");

  printf("Encode: TPose2DReference(KReferenceSLAM)...");
  A.SetPose2DReference(TPose2DReference(KReferenceSLAM));
  printf("OK.\n");

  printf("Encode: TSpeed2D(1.1,2.2)...");
  A.SetSpeed2D(TSpeed2D(1.1,2.2));
  printf("OK.\n");
  
  printf("\nDecoding...\n");
  // Do Decoding
  B.DecodeFrom(&bb);
  printf("Decode: Timestamp = (%d, %d)\n", 
	 B.GetTimestamp()->timestamp_s, 
	 B.GetTimestamp()->timestamp_us);
  
  printf("Decode: TPose2D = (%.1f, %.1f, %.1f)\n", 
	 B.GetPose2D()->x, 
	 B.GetPose2D()->y, 
	 B.GetPose2D()->a);

  printf("Decode: TPose3D = (%.1f, %.1f, %.1f, %.1f, %.1f, %.1f)\n", 
	 B.GetPose3D()->x, 
	 B.GetPose3D()->y, 
	 B.GetPose3D()->z,
         B.GetPose3D()->eul_x, 
	 B.GetPose3D()->eul_y, 
	 B.GetPose3D()->eul_z);

  printf("Decode: TVariance2D = (%.1f, %.1f, %.1f)\n", 
	 B.GetVariance2D()->x, 
	 B.GetVariance2D()->y, 
	 B.GetVariance2D()->a);
  printf("Decode: TVariance3D = (%.1f, %.1f, %.1f) (%.2f, %.2f, %.2f)\n", 
	 B.GetVariance3D()->x, 
	 B.GetVariance3D()->y, 
	 B.GetVariance3D()->z,
	 B.GetVariance3D()->eul_x, 
	 B.GetVariance3D()->eul_y, 
	 B.GetVariance3D()->eul_z);
  printf("Decode: TCommand = (%d)\n", 
	 B.GetCommand()->cmd);
  assert(B.GetCommand()->cmd == KCommandInitialize);

  printf("Decode: TPose2DReference = (%d)\n", 
	 B.GetPose2DReference()->ref);
  assert(B.GetPose2DReference()->ref == KReferenceSLAM);

  printf("Decode: TSpeed2D = (%f, %f)\n", 
	 B.GetSpeed2D()->speed,
	 B.GetSpeed2D()->angularSpeed);

  printf("\nTest complete.\n");
  

}
#endif
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
